/**
 * @author Yunfeng Bai
 *
 * 2012-12-1
 */
package collision;

import geometry.Box;
import geometry.Drawable;
import geometry.Triangle;
import geometry.TriangleMesh;

import java.util.ArrayList;
import java.util.List;

import javax.media.opengl.GL2;
import javax.vecmath.Matrix4d;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import simulation.ImpulseConstraintSolver;
import sun.reflect.generics.reflectiveObjects.NotImplementedException;
import util.Util;
import cern.colt.matrix.DoubleMatrix1D;
import cern.colt.matrix.DoubleMatrix2D;
import cern.colt.matrix.impl.DenseDoubleMatrix2D;
import cern.colt.matrix.linalg.EigenvalueDecomposition;


public class OBB extends BoundingVolume
{
	// Following variables are in world space.
	public Vector3d axes[] = new Vector3d[3]; // The three axes.
	public Point3d center = new Point3d();
	public double size[] = new double[3]; // The halfwidth in each axes.

	private Matrix4d toModelSpace = new Matrix4d();
	private Matrix4d toWorldSpace = new Matrix4d();
	
	private OBBTree tree;
	private long tick = 0;
	
	// Geometry data is in model space.
	private Drawable geometry;
	private TriangleMesh mesh;
	
	public static boolean isCollidingSlow1D(Point3d pa[], Point3d centerA, Point3d pb[], Point3d centerB, Vector3d centerDiff, Vector3d axis)
	{
		double t = Math.abs(centerDiff.dot(axis));
		// Get the radii for A.
		double radiiA = -1;
		for (int i = 0; i < 8; i++)
		{
			Vector3d diff = new Vector3d(pa[i]);
			diff.sub(centerA);
			double d = Math.abs(diff.dot(axis));
			if (d > radiiA) radiiA = d;
		}
		// Get the radii for B.
		double radiiB = -1;
		for (int i = 0; i < 8; i++)
		{
			Vector3d diff = new Vector3d(pb[i]);
			diff.sub(centerB);
			double d = Math.abs(diff.dot(axis));
			if (d > radiiB) radiiB = d;
		}
		return t < radiiA + radiiB;
	}
	
	public static boolean isCollidingSlow(OBB obb1, Matrix4d transform1inv, OBB obb2, Matrix4d transform2)
	{
		Vector3d bAxes[] = new Vector3d[3];
		for (int i = 0; i < 3; i++) bAxes[i] = new Vector3d(obb2.axes[i]);
		
		// Transform axes from B's frame to world space.
		for (int i = 0; i < 3; i++) transform2.transform(bAxes[i]);
		
		// Transform axes from world space to A's frame.
		for (int i = 0; i < 3; i++) transform1inv.transform(bAxes[i]);
		
		// Transform B's center point from B's frame to A's frame.
		Point3d bCenter = new Point3d(obb2.center);
		transform2.transform(bCenter);
		transform1inv.transform(bCenter);
		
		// Get all points of A and B.
		Point3d pa[] = new Point3d[8];
		Point3d pb[] = new Point3d[8];
		Vector3d v[] = new Vector3d[3];
		for (int i = 0; i < 3; i++) v[i] = new Vector3d();
		int pos = 0;
		for (int i = 0; i < 2; i++)
			for (int j = 0; j < 2; j++)
				for (int k = 0; k < 2; k++)
				{
					// Get point for A.
					for (int l = 0; l < 3; l++)
						v[l].set(obb1.axes[l]);
					v[0].scale((i * 2 - 1) * obb1.size[0]);
					v[1].scale((j * 2 - 1) * obb1.size[1]);
					v[2].scale((k * 2 - 1) * obb1.size[2]);
					pa[pos] = new Point3d(obb1.center);
					pa[pos].add(v[0]);
					pa[pos].add(v[1]);
					pa[pos].add(v[2]);
					
					// Get point for B.
					for (int l = 0; l < 3; l++)
						v[l].set(bAxes[l]);
					v[0].scale((i * 2 - 1) * obb2.size[0]);
					v[1].scale((j * 2 - 1) * obb2.size[1]);
					v[2].scale((k * 2 - 1) * obb2.size[2]);
					pb[pos] = new Point3d(bCenter);
					pb[pos].add(v[0]);
					pb[pos].add(v[1]);
					pb[pos].add(v[2]);
					
					pos++;
				}
		
		Vector3d d = new Vector3d(obb1.center);
		d.sub(bCenter);
		
		// Project onto 3 axes from A.
		for (int i = 0; i < 3; i++)
		{
			if (!isCollidingSlow1D(pa, obb1.center, pb, bCenter, d, obb1.axes[i])) return false;
		}
		// Project onto 3 axes from B.
		for (int i = 0; i < 3; i++)
		{
			if (!isCollidingSlow1D(pa, obb1.center, pb, bCenter, d, bAxes[i])) return false;
		}
		// Project onto 9 axes from the cross product of a pair of edges from A x B.
		for (int i = 0; i < 3; i++)
			for (int j = 0; j < 3; j++)
			{
				Vector3d axis = new Vector3d();
				axis.cross(obb1.axes[i], bAxes[j]);
				if (axis.length() < 1e-8) continue;
				
				if (!isCollidingSlow1D(pa, obb1.center, pb, bCenter, d, axis)) return false;
			}
		
		return true;
	}
	
	/**
	 * Collision test from separating axis theorem.
	 * See "OBBTree: A Hierarchical Structure for Rapid Interference Detection" and
	 * more details in "Collision Queries using Oriented Bounding Boxes" from S. Gottschalk.
	 * 
	 * @param transform1inv transform1inv transforms from world space to A's frame.
	 * @param transform2	transform2 transforms from B's frame to world space.
	 */
	public static boolean isColliding(OBB obb1, Matrix4d transform1inv, OBB obb2, Matrix4d transform2)
	{
		Vector3d bAxes[] = new Vector3d[3];
		for (int i = 0; i < 3; i++) bAxes[i] = new Vector3d(obb2.axes[i]);
		
		// Transform axes from B's frame to world space.
		for (int i = 0; i < 3; i++) transform2.transform(bAxes[i]);
		
		// Transform axes from world space to A's frame.
		for (int i = 0; i < 3; i++) transform1inv.transform(bAxes[i]);
		
		// Rotate to replace the canonical basis by A's axes.
		// M = (U_A, V_A, W_A)^-1 = (U_A, V_A, W_A)^T
		Vector3d temp = new Vector3d();
		for (int i = 0; i < 3; i++)
		{
			for (int j = 0; j < 3; j++)
			{
				Util.setA(temp, j, obb1.axes[j].dot(bAxes[i]));
			}
			bAxes[i].set(temp);
		}
		
		// Transform B's center point from B's frame to A's frame.
		Point3d bCenter = new Point3d(obb2.center);
		transform2.transform(bCenter);
		transform1inv.transform(bCenter);
		
		// Transform the translation vector after change of basis to A's axes.
		Vector3d t = new Vector3d();
		t.sub(obb1.center, bCenter);
		for (int i = 0; i < 3; i++)
			Util.setA(temp, i, obb1.axes[i].dot(t));
		t.set(temp);
		
		// Check three axes from A.
		for (int i = 0; i < 3; i++)
		{
			double tl = Math.abs(Util.getA(t, i));
			double d = obb1.size[i];
			for (int j = 0; j < 3; j++)
				d += Math.abs(obb2.size[j] * Util.getA(bAxes[j], i));
			if (tl > d) return false;
		}
		
		// Check three axes from B.
		for (int i = 0; i < 3; i++)
		{
			double tl = Math.abs(t.dot(bAxes[i]));
			double d = obb2.size[i];
			for (int j = 0; j < 3; j++)
				d += Math.abs(obb1.size[j] * Util.getA(bAxes[i], j));
			if (tl > d) return false;
		}
		
		// Check nine axes which are the cross products of each axes from A and B.
		for (int i = 0; i < 3; i++)
			for (int j = 0; j < 3; j++)
			{
				double tl = Math.abs(Util.getA(t, (i + 1) % 3) * Util.getA(bAxes[j], (i + 2) % 3)
						  		   - Util.getA(t, (i + 2) % 3) * Util.getA(bAxes[j], (i + 1) % 3));
				double d = 0;
				for (int k = 0; k < 3; k++)
				{
					if (k != i) d += obb1.size[k] * Math.abs(Util.getA(bAxes[j], 3 - k - i));
					if (k != j) d += obb2.size[k] * Math.abs(Util.getA(bAxes[3 - j - k], i));
				}
				if (tl > d) return false;
			}
		
		return true;
	}
	
	public OBB() {}
	
	public OBB(List<Triangle> triangles, OBBTree tree)
	{
		List<Point3d> vertices = new ArrayList<Point3d>();
		for (Triangle t : triangles)
		{
			vertices.add(t.getA());
			vertices.add(t.getB());
			vertices.add(t.getC());
		}
		init(vertices);
		this.tree = tree;
	}
	
	/**
	 * Build a tight OBB with PCA on vertices.
	 */
	public void init(List<Point3d> vertices)
	{
		// Get the mass center of all vertices.
		Point3d massCenter = new Point3d();
		for (Point3d p : vertices)
		{
			massCenter.add(p);
		}
		massCenter.scale(1.0d / vertices.size());
		
		// Calculate the covariance matrix based on vertices.
		// (TODO make the calculation base on triangles for better result)
		double com[][] = new double[3][];
		
		for (int i = 0; i < 3; i++) com[i] = new double[3];
			
		for (Point3d p : vertices)
		{
			com[0][0] += (p.x - massCenter.x) * (p.x - massCenter.x);
			com[1][1] += (p.y - massCenter.y) * (p.y - massCenter.y);
			com[2][2] += (p.z - massCenter.z) * (p.z - massCenter.z);
			com[0][1] += (p.x - massCenter.x) * (p.y - massCenter.y);
			com[0][2] += (p.x - massCenter.x) * (p.z - massCenter.z);
			com[1][2] += (p.y - massCenter.y) * (p.z - massCenter.z);
		}
		com[1][0] = com[0][1];
		com[2][0] = com[0][2];
		com[2][1] = com[1][2];
		for (int i = 0; i < 3; i++)
			for (int j = 0; j < 3; j++)
				com[i][j] /= vertices.size();
		
		// Calculate the eigenvectors and eigenvalues with Colt.
		// Use the three eigenvectors as the orientation of the OBB.
		DoubleMatrix2D coltMatrix = new DenseDoubleMatrix2D(com);
		EigenvalueDecomposition ed = new EigenvalueDecomposition(coltMatrix);
		DoubleMatrix2D eigenVectors = ed.getV();
		center.set(0, 0, 0);
		for (int i = 0; i < 3; i++)
		{
			DoubleMatrix1D vec = eigenVectors.viewColumn(i);
			axes[i] = new Vector3d();
			axes[i].set(vec.toArray());
			
			// Project points onto each axes to get the extends.
			double minExtend = Double.POSITIVE_INFINITY, maxExtend = Double.NEGATIVE_INFINITY;
			for (Point3d p : vertices)
			{
				double v = p.x * vec.get(0) + p.y * vec.get(1) + p.z * vec.get(2);
				if (v < minExtend) minExtend = v;
				if (v > maxExtend) maxExtend = v;
			}
			size[i] = (maxExtend - minExtend) / 2;
			
			double mid = minExtend + size[i];
			center.x += axes[i].x * mid;
			center.y += axes[i].y * mid;
			center.z += axes[i].z * mid;
		}
		
		toModelSpace.setIdentity();
		toWorldSpace.setIdentity();
	}
	
	@Override
	public void build(GL2 gl)
	{
		// The drawing list is built in model space.
		if (mesh == null)
		{
			Box box = new Box(size[0] * 2, size[1] * 2, size[2] * 2);
			mesh = box.getTriangleMesh();
		}
		
		_listNum = gl.glGenLists(1);
		gl.glNewList(_listNum, GL2.GL_COMPILE);
		
		gl.glPolygonMode(GL2.GL_FRONT_AND_BACK, GL2.GL_LINE);
		
		gl.glTranslated(center.x, center.y, center.z);
		
		// Do the rotation.
		gl.glMultMatrixd(Util.axesToArray(axes), 0);
		for(Triangle t : mesh._triangles)
			t.draw(gl);
		
		gl.glPolygonMode(GL2.GL_FRONT_AND_BACK, GL2.GL_FILL);
		
		gl.glEndList();
	}
	
	public void directDraw(GL2 gl)
	{
		// This method is called by OBBTree.buildTriangleMesh, which is built in model space.
		if (mesh == null)
		{
			Box box = new Box(size[0] * 2, size[1] * 2, size[2] * 2);
			mesh = box.getTriangleMesh();
		}
		
		gl.glPushMatrix();
		
		Point3d centerInWorldSpace = new Point3d(center);
		Vector3d[] axesInWorldSpace = new Vector3d[3];
		axesInWorldSpace[0] = new Vector3d(axes[0]);
		axesInWorldSpace[1] = new Vector3d(axes[1]);
		axesInWorldSpace[2] = new Vector3d(axes[2]);
		
		// Transform everything from world space to model space.
		toModelSpace.transform(centerInWorldSpace);
		toModelSpace.transform(axesInWorldSpace[0]);
		toModelSpace.transform(axesInWorldSpace[1]);
		toModelSpace.transform(axesInWorldSpace[2]);
		
		// Do the transformations.
		gl.glTranslated(centerInWorldSpace.x, centerInWorldSpace.y, centerInWorldSpace.z);
		gl.glMultMatrixd(Util.axesToArray(axesInWorldSpace), 0);
		
		for(Triangle t : mesh._triangles)
			t.draw(gl);
		
		gl.glPopMatrix();
	}

	@Override
	public TriangleMesh getTriangleMesh()
	{
		if (mesh == null)
		{
			Box box = new Box(size[0] * 2, size[1] * 2, size[2] * 2);
			mesh = box.getTriangleMesh();
		}
		
		return mesh;
	}
	
	@Override
	public boolean isColliding(BoundingVolume bv)
	{
		if (!(bv instanceof OBB))
			throw new NotImplementedException();
		
		OBB obb2 = (OBB)bv;
		Vector3d bAxes[] = new Vector3d[3];
		for (int i = 0; i < 3; i++) bAxes[i] = new Vector3d(obb2.axes[i]);
		
		// Rotate to replace the canonical basis by A's axes.
		// M = (U_A, V_A, W_A)^-1 = (U_A, V_A, W_A)^T
		Vector3d temp = new Vector3d();
		for (int i = 0; i < 3; i++)
		{
			for (int j = 0; j < 3; j++)
			{
				Util.setA(temp, j, axes[j].dot(bAxes[i]));
			}
			bAxes[i].set(temp);
		}
		
		// Transform B's center point from B's frame to A's frame.
		Point3d bCenter = new Point3d(obb2.center);
		
		// Transform the translation vector after change of basis to A's axes.
		Vector3d t = new Vector3d();
		t.sub(center, bCenter);
		for (int i = 0; i < 3; i++)
			Util.setA(temp, i, axes[i].dot(t));
		t.set(temp);
		
		// Check three axes from A.
		for (int i = 0; i < 3; i++)
		{
			double tl = Math.abs(Util.getA(t, i));
			double d = size[i];
			for (int j = 0; j < 3; j++)
				d += Math.abs(obb2.size[j] * Util.getA(bAxes[j], i));
			if (tl > d) return false;
		}
		
		// Check three axes from B.
		for (int i = 0; i < 3; i++)
		{
			double tl = Math.abs(t.dot(bAxes[i]));
			double d = obb2.size[i];
			for (int j = 0; j < 3; j++)
				d += Math.abs(size[j] * Util.getA(bAxes[i], j));
			if (tl > d) return false;
		}
		
		// Check nine axes which are the cross products of each axes from A and B.
		for (int i = 0; i < 3; i++)
			for (int j = 0; j < 3; j++)
			{
				double tl = Math.abs(Util.getA(t, (i + 1) % 3) * Util.getA(bAxes[j], (i + 2) % 3)
						  		   - Util.getA(t, (i + 2) % 3) * Util.getA(bAxes[j], (i + 1) % 3));
				double d = 0;
				for (int k = 0; k < 3; k++)
				{
					if (k != i) d += size[k] * Math.abs(Util.getA(bAxes[j], 3 - k - i));
					if (k != j) d += obb2.size[k] * Math.abs(Util.getA(bAxes[3 - j - k], i));
				}
				if (tl > d) return false;
			}
		
		return true;
	}
	
	/**
	 * The manhattan distance to p,
	 * positive if the point is out of the box; negative if the point is inside the box.
	 */
	public double manhattanDistanceToPoint(Point3d p)
	{
		// Convert p into local coordinates.
		Vector3d v = new Vector3d(p);
		v.sub(center);
		
		double maxDist = Double.NEGATIVE_INFINITY;
		for (int i = 0; i < 3; i++)
		{
			double coord = v.dot(axes[i]);
			double dist;
			if (coord < 0)
				dist = -size[i] - coord;
			else
				dist = coord - size[i];
			if (dist > maxDist) maxDist = dist;
		}
		return maxDist;
	}

	@Override
	public BoundingBox getBoundingBox()
	{
		// There is no need to get the bounding box for OBB.
		return null;
	}

	public void update()
	{
		if (tree == null)
		{
			int k = 0;
		}
		if (tree.getTick() == tick) return;
		tick = tree.getTick();
		
		long time = System.nanoTime();
		
		Matrix4d t = new Matrix4d();
		t.mul(tree.getToWorldSpaceMatrix(), toModelSpace);
		t.transform(center);
		t.transform(axes[0]);
		t.transform(axes[1]);
		t.transform(axes[2]);
		
		toModelSpace = tree.getToModelSpaceMatrix();
		toWorldSpace = tree.getToWorldSpaceMatrix();
		
		ImpulseConstraintSolver.updateOBBMatrixTime += System.nanoTime() - time;
	}
	
	public Matrix4d getToWorldSpaceMatrix()
	{
		return toWorldSpace;
	}
	
	public Matrix4d getToModelSpaceMatrix()
	{
		return toModelSpace;
	}
}
